; **********************************************************************
;               Code for means-ends problem solving
; **********************************************************************

; The original CHAIN-BACKWARD has been broken down to PERFORM-RETRIEVAL
; and CHAIN-BACKWARD below, to separate out problem-solver portions from
; regular execution procedures. PERFORM-RETRIEVAL is in INTERPRETER.LISP.

; CHAIN-BACKWARD inputs an unsatisfied goal and a goal stack in which 
; it is the first element. The function tries to backward chain off
; known skills and, if one is retrieved, it adds this to the goal. 
; If no skills are retrieved, it backward chains off the definition
; of the goal, selects an unsatisfied subgoal at random, and pushes 
; this onto the goal stack. 

; This version of CHAIN-BACKWARD prefers chaining off skills over chaining
; off concepts, but this can cause problems in some domains, so we need a
; better long-term solution. Commented lines give the ability to select
; randomly between skill and concept chaining when both are possible. 

(defun chain-backward (goal goal-stack)
  (let* ((objective (goal-objective goal))
	 (skill (chain-on-skill objective goal-stack))
	 (chained (chain-on-concept objective goal goal-stack)))
    (cond ((not (null skill))
	   (print-selecting-skill skill)
	   (setf (goal-chaintype goal) 'skill)
	   (setf (goal-intention (car goal-stack)) skill)
	   (cond ((not (satisfied-with-partial
			(car (sinstance-start skill)) cstm*))
		  (pushd (make-goal :objective
				    (car (sinstance-start skill))
				    :type 'PRECOND)
			 goal-stack)
		  'PRECOND-PUSHED)
		 (t
		  'SKILL-SELECTED)))
	  ((not (null chained))
	   (print-selecting-concept (car chained))
	   (setf (goal-chaintype goal) 'concept)
	   (setf (goal-achieved goal) (rcopy (cdr chained)))
	   (pushd (make-goal :objective (car chained)
			     :type 'SUBGOAL) goal-stack)
	   'SUBGOAL-PUSHED)
	  ((and (not (null skill)) (not (null chained)))
	   (cond ((zerop (random 2))
		  (print-selecting-skill skill)
		  (setf (goal-chaintype goal) 'skill)
		  (setf (goal-intention (car goal-stack)) skill)
		  (cond ((not (satisfied-with-partial
				    (car (sinstance-start skill))
				    cstm*))
			 (pushd (make-goal
				 :objective
				 (car (sinstance-start skill))
				 :type 'PRECOND)
				goal-stack)
			 'PRECOND-PUSHED)
			(t
			 'SKILL-SELECTED)))
		 (t (print-selecting-concept (car chained))
		    (setf (goal-chaintype goal) 'concept)
		    (setf (goal-achieved goal) (rcopy (cdr chained)))
		    (pushd (make-goal :objective (car chained)
				      :type 'SUBGOAL)
			   goal-stack)
		    'SUBGOAL-PUSHED)))
	  (t (print-backtracking2)
	     (fail-and-pop objective goal-stack)
	     'FAIL-AND-POPPED))))

; CHAIN-ON-CONCEPT inputs an unsatisfied goal literal (OBJECTIVE), the
; GOAL structure in which it occurs, and the GOAL-STACK in which that 
; is the first element. The function identifies which subconcepts are 
; unsatisfied, selects one at random that has not already failed and
; that does not appear in a parent goal, and pushes this subgoal onto 
; the goal stack. It returns a list that contains one of the unsatisfied 
; subconcept instances followed by all satisfied subconcept instances, 
; including negated ones.

; NOTE: This version deals only with POSITIVE goals and ignores
;       negative ones. These will be trickier, since it involves 
;       negating the subconcepts and achieving ANY of them, since 
;       this will satisfy the negated parent goal. 
; 
; NOTE: We should also extend this function to handle disjunctive 
;       concepts, which will probably mean modifying the goal stack 
;       to store concept instances with ids. 

(defun chain-on-concept (objective goal goal-stack)
  (unless (not (null objective)) (return-from chain-on-concept))
  (let* ((clause (cond ((eq (car objective) 'not)
			(cassoc (caadr objective) cltm*))
		       (t (cassoc (car objective) cltm*))))
;	 (clause (get-untried-clause (car objective)
;				     (goal-failed goal) cltm*))
	 (relations (concept-relations clause)))
    (cond ((eq (car objective) 'not) nil)
	  ((not (null relations))
	   (let* ((head (concept-head clause))
		  (bindings (mapcar #'(lambda (v i) (cons v i))
				    (cdr head) (cdr objective)))
		  (failed (goal-cfailed goal))
		  (subgoals (goal-subgoals goal))
		  (matched nil)
		  (unmatched nil))
	     (setq relations (subst-bindings bindings relations))
; (terpri)(princ "Goal: ")(princ objective)
; (terpri)(princ "Subgoals: ")(princ relations)
	     (do ((next (car relations) (car relations)))
		 ((null relations)
		  (cond ((null unmatched) nil)
			(t (print-concept-choices unmatched)
			   (cons (select-at-random unmatched) matched))))
		 (cond ((satisfied-with-partial next cstm*)
			(push next matched))
		       ((and (not (member next failed :test 'equal))
			     (not-achieved-before next subgoals)
			     (not-parent-goal next (cdr goal-stack)))
			(push next unmatched)))
		 (pop relations)))))))

(defun not-parent-goal (goal stack)
  (cond ((null stack) t)
	((consistent-head goal (goal-objective (car stack))) nil)
	(t (not-parent-goal goal (cdr stack)))))

(defun not-achieved-before (goal before)
  (not (member goal before :test 'equal)))

(defun get-untried-clause (predicate failed cltm)
  (cond ((null cltm) nil)
	((eq (car (concept-head (car cltm))) predicate)
	 (cond ((already-failed (car cltm) failed)
		(get-untried-clause predicate failed (cdr cltm)))
	       (t (car cltm))))
	(t (get-untried-clause predicate failed (cdr cltm)))))

; ALREADY-FAILED returns T (or something equivalent) if the list of FAILED 
; concept instances already contains an element with the same ID as CLAUSE. 

(defun already-failed (clause failed)
  (member (concept-id clause) failed :key #'cinstance-id))

(defun chain-on-skill (objective goal-stack)
  (let* ((skills (get-relevant-skills objective nil sltm*))
         (skills (remove-failed skills (goal-sfailed (car goal-stack))))
	 (skills (remove-unchainable skills))
	 (skills (remove-complex skills))
	 (subgoals (cond ((null (cadr goal-stack)) nil)
			 (t (mapcan #'expand-start
				    (goal-subgoals (cadr goal-stack))))))
	 (skills (cond ((not (null subgoals))
			(remove-parent-undoers skills subgoals))
		       (t skills)))
;	 (skills (remove-undoers skills goal-stack))
;	 (skills (remove-previous skills goal-stack))
	 (skills (remove-loops skills goal-stack)))
    (cond ((not (null skills))
	   (setq skills (remove-duplicates skills))
	   (let ((applicables (get-applicable-skills skills)))
	     (cond ((null applicables)
		    (print-skill-choices nil skills)
		    (select-at-random skills))
		   (t (print-skill-choices t applicables)
		      (select-at-random applicables))))))))

(defun get-applicable-skills (skills)
  (cond ((null skills) nil)
	((is-applicable (car skills))
	 (cons (car skills) (get-applicable-skills (cdr skills))))
	(t (get-applicable-skills (cdr skills)))))

(defun is-applicable (sinst)
;  (satisfied (sinstance-start sinst) cstm*))
  (satisfied-with-partial (car (sinstance-start sinst)) cstm*))

; GET-RELEVANT-SKILLS returns all the skills where GOAL appears in the
; objective field. This function would be more efficient if concepts 
; included pointers to the skills that achieve them.
; NOTE: This version backward chains off every skill clause, even when 
; two or more have the same head. 

(defun get-relevant-skills (goal next sltm)
  (cond ((null sltm) nil)
	((setq next (get-relevant-instances goal (car sltm)))
	 ;(terpri)(princ "Next:")(terpri)(princ next)
	 (append next (get-relevant-skills goal nil (cdr sltm))))
	(t (get-relevant-skills goal nil (cdr sltm)))))

(defun get-relevant-instances (goal clause)
  (let ((head (sclause-head clause))
	(id (sclause-id clause))
;	 (start (car (sclause-start clause)))
	(start (sclause-start clause))
	(percepts (object-variable-and-constants (sclause-percepts clause)))
	head-result bindings bset results)
    (if (heads-with-same-name goal clause)
	(setq head-result (match-heads (list goal) (list head) nil))
	(setq head-result (match-heads (expand-start goal)
				       (expand-start head)
				       nil)))
    (if (car head-result)
	(if (null (cdr head-result))
	    ; to take care of null bindings checker in do loop below
	    (setq bindings '(nil))
	    (setq bindings (cdr head-result))))
;    (terpri)(princ "bindings: ")(princ bindings)
    (do ((bnext (car bindings) (car bindings)))
	((null bindings) results)
      (setq bnext (ground-binding bnext))
;      (terpri)(princ "bnext: ")(princ bnext)
      (setq bset (mapcar #'car (match-pconds percepts pstm* nil bnext)))
;      (terpri)(princ "bset: ")(princ bset)
      (setq bset (mapcar #'(lambda (b)
			     (make-sinstance :goal (subst-bindings b head)
					     :id id :bindings b
					     :start (subst-bindings b start)))
			 bset))
;      (terpri)(princ "bset: ")(princ bset)
      (setq results (append bset results))
      (pop bindings))))

;This function was used in GET-RELEVANT-INSTANCES before, but replaced with
;OBJECT-VARIABLE-AND-CONSTANTS to make the skill chaining choices smarter.
;(defun object-variables (percepts)
;  (mapcar #'(lambda (p) (list (car p) (cadr p))) percepts))

(defun object-variable-and-constants (percepts)
  (mapcar #'(lambda (p)
	      (append (list (car p) (cadr p))
		      (select-constant-attributes (cddr p))))
	  percepts))

(defun select-constant-attributes (list)
  (unless (null list)
    (let ((attribute (car list))
	  (value (cadr list)))
      (if (not (variablep value))
	  (append (list attribute value)
		  (select-constant-attributes (cddr list)))
	  (select-constant-attributes (cddr list))))))

(defun ground-binding (x)
  (cond ((null x) nil)
	((variablep (cdar x))
	 (ground-binding (cdr x)))
	(t (cons (car x) (ground-binding (cdr x))))))

; REMOVE-FAILED inputs a list of skill instances (each with associated
; start conditions) and returns only those not contained in FAILED, the
; contents of the :failed field for the current goal.

(defun remove-failed (skills failed)
  (cond ((null skills) nil)
        ((member (car skills) failed :test 'same-sinstance)
         (remove-failed (cdr skills) failed))
        (t (cons (car skills) (remove-failed (cdr skills) failed)))))

(defun same-sinstance (i j)
  (and (eq (sinstance-id i) (sinstance-id j))
       (equal (sinstance-goal i) (sinstance-goal j))
       (equal (sinstance-bindings i) (sinstance-bindings j))))

; REMOVE-UNCHAINABLE inputs a list of skill instances (each with associated
; start conditions) and returns only those whose skills are not included
; in the UNCHAINABLE* list.

(defun remove-unchainable (skills)
  (cond ((null skills) nil)
        ((any-unchainable-concept
	  (expand-start (car (sinstance-start (car skills)))))
         (remove-unchainable (cdr skills)))
        (t (cons (car skills) (remove-unchainable (cdr skills))))))

(defun any-unchainable-concept (clist)
  (cond ((null clist) nil)
;       ((member (car clist) cstm* :test #'equal)
	((satisfied-with-partial (car clist) cstm*)
         (any-unchainable-concept (cdr clist)))
	((rsatisfied (car clist) unchainable*) t)
        (t (any-unchainable-concept (cdr clist)))))

; RSATISFIED, RSATISFIED-POSITIVE, and RSATISFIED-NEGATIVE are the 
; same as SATISFIED, SATISFIED-POSITIVE, and SATISFIED-NEGATIVE, 
; except they see whether a belief matches against any of a set of
; patterns, rather than the reverse. 

(defun rsatisfied (belief patterns)
  (cond ((eq (car belief) 'not)
	 (rsatisfied-negative (cadr belief) patterns))
	(t (rsatisfied-positive belief patterns))))

(defun rsatisfied-positive (belief patterns)
  (cond ((null patterns) nil)
        ((car (bmatches (car patterns) belief nil)) t)
        (t (rsatisfied-positive belief (cdr patterns)))))

(defun rsatisfied-negative (belief patterns)
  (cond ((null patterns) t)
        ((car (bmatches (car patterns) belief nil)) nil)
	(t (rsatisfied-negative belief (cdr patterns)))))

; REMOVE-COMPLEX inputs a list of skill instances (each with associated
; start conditions) and returns only that are either primitive skills
; or learned skills for which the start conditions are fully satisfied.

; Modified version of REMOVE-COMPLEX that removes complex skills even
; when their start conditions are matched, since if they participate
; in an applicable path, they will have already been found.

(defun remove-complex (skills)
  (cond ((null skills) nil)
        ((learned-skill (car skills))
         (remove-complex (cdr skills)))
        (t (cons (car skills) (remove-complex (cdr skills))))))

#| NOT USED NOW
; REMOVE-UNDOERS returns only those skills that do not clobber a goal
; that appears in the subgoals field of an entry later/lower in the
; goal stack. This requires checking the effects of a skill instance
; and seeing which literals they would make untrue. Not yet implemented.
; For a given skill instance, find all the negated literals it would
; cause to hold. If any of these match against elements already achieved
; earlier in the goal stack, then remove this instance from consideration.

(defun remove-undoers (skills goal-stack)
  (cond ((null skills) nil)
        ((would-undo (car skills) goal-stack)
         (remove-undoers (cdr skills) goal-stack))
        (t (cons (car skills) (remove-undoers (cdr skills) goal-stack)))))
			
; WOULD-UNDO returns T if any partially instantiated negatives implied
; by SKILL match against any goals that occur in the subgoals field of 
; an element in GOAL-STACK, i.e., if any subgoals that have already been
; achieved would be clobbered by its execution. 

(defun would-undo (iskill goal-stack)
  (let* ((changes (lookahead iskill cstm*))
         (after (append (car changes)
                        (remove-nmatches (cadr changes) cstm*))))
    (would-undo-aux after goal-stack iskill)))

; WOULD-UNDO-AUX checks each member of the goal stack, returning T if 
; any subgoals already achieved in a given member are not in AFTER, 
; the state that would result if a skill were executed. 

(defun would-undo-aux (after goal-stack iskill)
  (cond ((null goal-stack) nil)
        ((not (subsetp (goal-subgoals (car goal-stack))
                       after :test 'equal))
         (cond ((not (null mtrace*))
                (terpri)
		(format t "*** Detected clobbered goal ~A." iskill))) t)
        (t (would-undo-aux after (cdr goal-stack) iskill))))
|#

(defun remove-parent-undoers (skills subgoals)
  (cond ((not (null skills))
	 (let* ((sinst (car skills))
		(clause (car (member (sinstance-id sinst) 
				     sltm* :key 'sclause-id)))
		(bindings (sinstance-bindings sinst))
		(head (subst-bindings bindings (sclause-head clause)))
		(last (subst-bindings bindings
				     (car (last (sclause-subgoals clause))))))
	   (cond ((or (would-locally-undo (expand-start head) subgoals)
		      (would-locally-undo (expand-start last) subgoals))
		  (remove-parent-undoers (cdr skills) subgoals))
		 (t (cons sinst
			  (remove-parent-undoers (cdr skills) subgoals))))))))

(defun remove-local-undoers (bindings-set clause subgoals)
  (cond ((not (null bindings-set))
         (let* ((last (subst-bindings (car bindings-set)
                                  (car (last (sclause-subgoals clause))))))
           (cond ((and (not (null subgoals))
		       (would-locally-undo (expand-start last) subgoals))
                  (remove-local-undoers (cdr bindings-set) clause subgoals))
                 (t (cons (car bindings-set) 
                          (remove-local-undoers (cdr bindings-set)
						clause subgoals))))))
        (t nil)))

(defun would-locally-undo (last subgoals)
  (cond ((null last) nil)
	((would-change (car last) subgoals) t)
	(t (would-locally-undo (cdr last) subgoals))))

(defun would-change (change achieved)
  (cond ((eq (car change) '*neg*)
	 (would-change-negative (cadr change) achieved))
	(t (would-change-positive change achieved))))

(defun would-change-negative (neg-change achieved)
  (cond ((null achieved) nil)
	((and (not (eq (caar achieved) '*neg*))
	      (car (bmatches neg-change (make-cinstance :head (car achieved))
			     nil))) t)
	(t (would-change-negative neg-change (cdr achieved)))))

(defun would-change-positive (pos-change achieved)
  (cond ((null achieved) nil)
	((and (eq (caar achieved) '*neg*)
	      (car (bmatches (cadar achieved) (make-cinstance :head pos-change)
			     nil))) t)
	(t (would-change-positive pos-change (cdr achieved)))))

#| NOT USED NOW
(defun remove-previous (skills goal-stack)
  (unless skills (return-from remove-previous nil))
  (let* ((proposal (copy-goal-stack goal-stack)))
    (setf (goal-intention (first proposal)) (caar skills))
    (setf (goal-chaintype (first proposal)) 'skill)
    (cond ((member proposal failures* :test #'goal-stack-equal)
           (cond ((not (null mtrace*))
                  (terpri)(format t "*** Detected previous failure ~A."
                                  (caar skills))))
           (remove-previous (cdr skills) goal-stack))
          (t (cons (car skills)
                   (remove-previous (cdr skills) goal-stack))))))
|#

; REMOVE-LOOPS inputs a list of skill instances that are candidates for
; skill chaining and the current goal stack. It returns only those skill
; instances for which the goal does not equal the goal of some intention
; already in the stack and for which the instantied start condition does
; not subsume an objective already in the stack. 

(defun remove-loops (skills goal-stack)
  (cond ((null skills) nil)
        ((or (member (sinstance-goal (car skills)) goal-stack
		     :key #'intention-goal :test #'equal)
;	     (member (sinstance-start (car skills)) goal-stack
	     (member (car (sinstance-start (car skills))) goal-stack
		     :key #'goal-objective :test #'heads-match))
         (remove-loops (cdr skills) goal-stack))
        (t (cons (car skills) (remove-loops (cdr skills) goal-stack)))))

(defun intention-goal (x)
  (let ((intention (goal-intention x)))
    (cond ((not (null intention))
	   (sinstance-goal intention)))))

#| ;NOT USED
; ALREADY-HOLD inputs an objective (a concept instance) and expands it 
; one level down to find subconcept instances that already hold in the 
; environment.

(defun already-hold (objective)
  (already-hold-aux (expand-one objective)))

(defun already-hold-aux (expanded)
  (cond ((null expanded) nil)
        ((member (car expanded) cstm* :test 'equal)
         (cons (car expanded)
               (already-hold-aux (cdr expanded))))
        (t (already-hold-aux (cdr expanded)))))

(defun expand-one (cinst)
 (let* ((cdef (cassoc (car cinst) cltm*))
        (positives (concept-positives cdef))
        (unchainables (intersection positives unchainable*
                                    :test (lambda (x y) (eq (car x) y))))
        (bindings (cdr (bmatches (concept-head cdef) cinst nil)))
        (bindings (mapcar #'car
			  (match-bconds unchainables cstm* nil bindings)))
        (bindings (select-at-random bindings))
        (positives (subst-bindings bindings (concept-positives cdef)))
        (negatives (subst-bindings bindings (concept-negatives cdef))))
   (append positives negatives)))
|#

; Need to add this test for looping subgoals back in at some point. 

(defun repeated-subgoal (cinsts goal-stack)
  (let ((flag nil))
    (do ((cinst (car cinsts) (car cinsts)))
        ((or (null cinsts) flag) flag)
        (mapc #'(lambda (stack)
                  (if (equal (goal-objective stack) cinst)
                      (setq flag t)))
              goal-stack)
        (pop cinsts))))

(defun copy-goal-stack (goal-stack)
  (cond ((null goal-stack) nil)
        (t (cons (copy-goal (car goal-stack))
                 (copy-goal-stack (cdr goal-stack))))))

(defun goal-equal (x y)
  (and (equal (goal-objective x) (goal-objective y))
       (equal (goal-skills x) (goal-skills y))
       (equal (goal-intention x) (goal-intention y))
       (equal (goal-chaintype x) (goal-chaintype y))))

(defun goal-stack-equal (x y)
  (cond ((not (= (length x) (length y))) nil)
        ((null x) t)
        ((goal-equal (car x) (car y))
         (goal-stack-equal (cdr x) (cdr y)))
        (t nil)))

; NO-SSTORE-AND-POP is like SSTORE-AND-POP except that it does not learn
; a new hierarchical skill. 

(defun no-sstore-and-pop (goal goal-stack)
  (let* ((intention (goal-intention goal))
         (stack-entry (goal-path goal))
         (objective (goal-objective goal))
         (supergoal (cadr goal-stack)))
    (cond ((not (null stack-entry))
	   (setf (goal-path goal) nil)))
    (cond ((not (null supergoal))
	   (store-with-supergoal (goal-chaintype supergoal) intention
                                 objective supergoal)
           (setf (goal-sfailed supergoal) nil)
           (popd goal-stack))
	  (t (setf (goal-chaintype goal) nil)
             (setf (goal-intention goal) nil)))))

; NO-CSTORE-AND-POP is like CSTORE-AND-POP except that it does not learn
; a new hierarchical skill. 

(defun no-cstore-and-pop (goal goal-stack)
  (let* ((skills (reverse (mapcar #'sinstance-goal (goal-skills goal))))
         (objective (goal-objective goal))
         (bindings (reverse (generate-bindings (cons objective skills) nil)))
         (iargs (mapcar 'car bindings))
         (supergoal (cadr goal-stack))
	 ;What is the below? Why do we need it? - 21MAY08, DKC
         (new-instance
	    (make-sinstance :goal objective :id 0 :bindings bindings)))
    (cond ((not (null supergoal))
           (store-with-supergoal (goal-chaintype supergoal) new-instance
                                 (goal-objective goal) supergoal)
           (popd goal-stack))
          (t (setf (goal-chaintype goal) nil)
             (setf (goal-skills goal) nil)
             (setf (goal-subgoals goal) nil)
             (setf (goal-achieved goal) nil)))))

; FAIL-AND-POP inputs an objective and the goal stack in which it is
; the first element (actually the :objective field of this element). 
; If the system has executed any skills while pursuing this goal, it
; cannot backtrack over them, so it either halts or clears all but the
; top-level goal and continues from the current state, depending on 
; whether HALT-ON-FAILURE* is T or NIL. Otherwise, if this is not the 
; top-level (last) element on the goal stack, the function pops the 
; stack and stores, with the parent stack element, the current objective 
; or its associated skill as having failed. 

(defun fail-and-pop (objective goal-stack)
  (cond ((not (null (goal-skills (car goal-stack))))
	 (cond ((not (null halt-on-failure*))
		(print-giving-up)
		(setf halt* 'failure))
	       (t (let ((last (car (last goal-stack))))
		    (setf (car goal-stack) last)
		    (setf (cdr goal-stack) nil)
		    (setf (goal-chaintype last) nil)
		    (setf (goal-intention last) nil)
		    (setf (goal-path last) nil)
		    (setf (goal-skills last) nil)
		    (setf (goal-subgoals last) nil)
		    (setf (goal-achieved last) nil)
		    (setf (goal-sfailed last) nil)
		    (setf (goal-cfailed last) nil))
		  (print-continuing))))
	(t (let ((supergoal (cadr goal-stack)))
	     (cond ((not (null supergoal))
		    (popd goal-stack)
		    (cond ((eq (goal-chaintype supergoal) 'skill)
			   (setf (goal-chaintype supergoal) nil)
			   (setf (goal-sfailed supergoal)
				 (cons (goal-intention supergoal)
				       (goal-sfailed supergoal)))
			   (setf (goal-chaintype supergoal) nil)
			   (setf (goal-intention supergoal) nil))
			  (t (setf (goal-cfailed supergoal)
				   (cons objective (goal-cfailed supergoal)))
			     (setf (goal-chaintype supergoal) nil))))
		   (t
		    (incf failcount*)
	            (cond ((>= failcount* failure-limit*)
			   (setq halt* 'NOSKILL)
			   (store-crash-log)
			   ;(get-advice)
			   (setf (goal-chaintype (car goal-stack)) nil)
			   (setf (goal-sfailed (car goal-stack)) nil)
			   (setf (goal-chaintype (car goal-stack)) nil)
			   (setf (goal-intention (car goal-stack)) nil)
			   (setf (goal-cfailed (car goal-stack)) nil)
			   (setf (goal-achieved (car goal-stack)) nil)
			   ;(setq advicegiven T)
			   (setq failcount* 0))))))))
  nil)

(defun store-with-supergoal (type iskill objective supergoal)
  (cond ((eq type 'skill)
	 (setf (goal-precursor supergoal) iskill))
        ((or (eq type 'concept)
	     (eq type 'feature))
         (setf (goal-skills supergoal)
               (cons iskill (goal-skills supergoal)))
         (setf (goal-subgoals supergoal)
               (cons objective (goal-subgoals supergoal))))))

; POPD and PUSHD remove the top element and add an element to the top 
; of a stack, respectively. Both assume NIL at the bottom of the stack.

(defun popd (stack)
  (let ((i (cdr stack)))
        (rplaca stack (car i))
        (rplacd stack (cdr i))))

(defun pushd (new stack)
  (let ((i (copy-list stack)))
       (rplacd stack i)
       (rplaca stack new)))

(defun reinitialize-stack (stack)
  (rplaca stack (car (last stack)))
  (rplacd stack nil))
